#include <arpa/inet.h>
#include <netinet/in.h>
#include <cstdlib>
#include <sys/time.h>

using namespace std;

#include "SelectLoop.h"

SelectLoop::SelectLoop() :
	wakeups(), sockets(), max_fd(0), max_max_len(1), zero(), wakeup(), from(), fromlen(), buf(NULL)
{
	buf = malloc(1);
	fromlen = sizeof(from);
	zero.tv_sec = 0; zero.tv_usec = 0;
}

SelectLoop::WakeupInterval::WakeupInterval(unsigned int ms, TimeoutHandler *h) :
	interval(), last_wakeup(), time_to_next_wakeup(), handle(h)
{
	interval.tv_sec = 0;	interval.tv_usec = 1000 * ms;
	gettimeofday(&last_wakeup, NULL);	// To avoid immediate double send
}

SelectLoop::WakeupInterval::WakeupInterval(const WakeupInterval& o) :
	interval(o.interval), last_wakeup(o.last_wakeup), time_to_next_wakeup(o.time_to_next_wakeup), handle(o.handle) 
{}

SelectLoop::ListenSocket::ListenSocket(int sock, int max_len, SocketHandler *h) : sock(sock), max_len(max_len), handle(h) { }
SelectLoop::ListenSocket::ListenSocket(const ListenSocket& o): sock(o.sock), max_len(o.max_len), handle(o.handle) { }

unsigned int SelectLoop::add_wakeup_interval(unsigned int ms, TimeoutHandler *handle)
{
	wakeups.push_back(WakeupInterval(ms, handle));
	return wakeups.size() - 1;
}

void SelectLoop::reset_wakeup_interval(unsigned int identifier)
{
	gettimeofday(&wakeups[identifier].last_wakeup, NULL);
}

unsigned int SelectLoop::add_socket(int sock, int max_len, SocketHandler *handle)
{
	sockets.push_back(ListenSocket(sock, max_len, handle));
	
	if (sock > max_fd)
		max_fd = sock;

	if (max_len > max_max_len)
	{
		max_max_len = max_len;
		
		free(buf);		// Free old buffer, we're replacing it with a new one
		buf = malloc(max_len);
	}
	
	return sockets.size() - 1;
}

void SelectLoop::run()
{
	fd_set readfds;
	struct timeval now;
	
	FD_ZERO(&readfds);
	
	for (unsigned i = 0; i < sockets.size(); i ++)
		FD_SET(sockets[i].sock, &readfds);
	
	wakeup.tv_sec = 999; wakeup.tv_usec = 0;
	
	gettimeofday(&now, NULL);
	
	// Compute time until next wake up for each wake-up interval
	for (unsigned i = 0; i < wakeups.size(); i ++)
		wakeups[i].time_to_next_wakeup = tv_max(tv_difference(tv_difference(wakeups[i].last_wakeup, now), wakeups[i].interval), zero);
	
	for (unsigned i = 0; i < wakeups.size(); i ++)
		wakeup = tv_min(wakeup, wakeups[i].time_to_next_wakeup);
	
	int nfds = select(max_fd + 1, &readfds, NULL, NULL, &wakeup);
	
	if (nfds > 0)
	{
		for (unsigned i = 0; i < sockets.size(); i ++)
		{
			if (FD_ISSET(sockets[i].sock, &readfds))
			{
				recvfrom(sockets[i].sock, buf, sockets[i].max_len, 0, (struct sockaddr *) &from, &fromlen);
				(*(sockets[i].handle))(buf, (struct sockaddr *) &from);		// Call user provided handler
			}
		}
	}
	
	for (unsigned i = 0; i < wakeups.size(); i ++)
	{
		if (tv_compare(wakeups[i].interval, tv_difference(wakeups[i].last_wakeup, now)) <= 0)
		{
			gettimeofday(&wakeups[i].last_wakeup, NULL);
			(*(wakeups[i].handle))();										// Call user provided handler
		}
	}
}

struct timeval SelectLoop::tv_difference(struct timeval tv1, struct timeval tv2)
{	
	struct timeval tv;
	
	if (tv2.tv_usec < tv1.tv_usec) {
		tv.tv_sec = tv2.tv_sec - tv1.tv_sec - 1;
		tv.tv_usec = tv2.tv_usec - tv1.tv_usec + 1000000;
	} else {
		tv.tv_sec = tv2.tv_sec - tv1.tv_sec;
		tv.tv_usec = tv2.tv_usec - tv1.tv_usec;
	}
	
	return tv;
}

int SelectLoop::tv_compare(struct timeval tv1, struct timeval tv2)
{
	if (tv1.tv_sec == tv2.tv_sec && tv1.tv_usec == tv2.tv_usec)
		return 0;
	
	if (tv1.tv_sec < tv2.tv_sec || (tv1.tv_sec == tv2.tv_sec && tv1.tv_usec < tv2.tv_usec))
		return -1;
	
	return 1;
}

struct timeval SelectLoop::tv_min(struct timeval tv1, struct timeval tv2)
{	
	if (tv_compare(tv1, tv2) < 0)
		return tv1;
	
	return tv2;
}

struct timeval SelectLoop::tv_max(struct timeval tv1, struct timeval tv2)
{	
	if (tv_compare(tv1, tv2) < 0)
		return tv2;
	
	return tv1;
}
